function [ CBL ] = coarsealign_stat_extazm( fB, azm, azmAxis ) %#codegen
%COARSEALIGN_STAT_EXTAZM 利用重力和外部给出的方位角进行静基座初始粗对准
%
% Tips:
% # 参考坐标系L为NED
%
% Input Arguments:
% # fB: 3*1向量，加速度计测量到的比力（负的重力加速度）
% # azm: 标量，体坐标系B的azmaxis轴在水平面上的投影相对于北向的夹角（由北向绕顺时针转动到该投影为正），单位rad
% # azmAxis: 标量，1-3分别对应X、Y、Z，其它值默认为X轴
%
% Output Arguments:
% # CBL: 3*3矩阵，方向余弦矩阵
%
% References:
% # 《应用导航算法工程基础》“引入外部给出的方位基准时的解析粗对准算法”

if nargin < 3
    azmAxis = 1;
end

coder.extrinsic('error');
% 将B坐标系转换为B'坐标系，使得B系的azmaxis对应B'系的X轴
switch azmAxis
    case 2
        CBBPrime = [0 1 0; 0 0 1; 1 0 0]; % B系的X、Y、Z轴分别对应B'系的Z、X、Y轴
    case 3
        CBBPrime = [0 0 1; 1 0 0; 0 1 0]; % B系的X、Y、Z轴分别对应B'系的Y、Z、X轴
    otherwise
        CBBPrime = eye(3);
end
CBPrimeL = NaN(3);
fBPrime = CBBPrime * fB;
CBPrimeL(3, :) = -fBPrime' / norm(fBPrime);
q = 1 - CBPrimeL(3, 1)^2;
if (q>=eps) || (q<=-eps)
    CBPrimeL(1, 1) = cos(azm) * sqrt(q);
    CBPrimeL(2, 1) = sin(azm) * sqrt(q);
    CBPrimeL(1, 2) = (-CBPrimeL(2, 1)*CBPrimeL(3, 3) - CBPrimeL(1, 1)*CBPrimeL(3, 1)*CBPrimeL(3, 2)) / q;
    CBPrimeL(1, 3) = (CBPrimeL(2, 1)*CBPrimeL(3, 2) - CBPrimeL(1, 1)*CBPrimeL(3, 1)*CBPrimeL(3, 3)) / q;
    CBPrimeL(2, 2) = (CBPrimeL(1, 1) *CBPrimeL(3, 3) - CBPrimeL(2, 1)*CBPrimeL(3, 1)*CBPrimeL(3, 2)) / q;
    CBPrimeL(2, 3) = (-CBPrimeL(1, 1)*CBPrimeL(3, 2) - CBPrimeL(2, 1)*CBPrimeL(3, 1)*CBPrimeL(3, 3)) / q;
else
    error('azm axis is not horizontal axis!');
end
CBL = CBPrimeL * CBBPrime;
end